#include "Stepper_Motor.h"

int StepperCnt_buff[2] = {0,0}; //电机A 和 电机B
StepperMotor stepper_motor_A = {0, 0, 0, 0.0f, 0.0f};
StepperMotor stepper_motor_B = {1, 0, 0, 0.0f, 0.0f};

void StepperMotor_SetDirection(StepperMotor *motor, uint8_t direction)
{
    if (motor -> ID == 0)
    {
        if (direction == STEPPER_BACK || direction == 0)
        {
            motor->direction = STEPPER_BACK; // 反转
            DL_GPIO_clearPins(Stepper_Motor_PORT, Stepper_Motor_DIR1_PIN);
        }
        else if (direction == STEPPER_FRONT || direction == 1)
        {
            motor->direction = STEPPER_FRONT; // 正转
            DL_GPIO_setPins(Stepper_Motor_PORT, Stepper_Motor_DIR1_PIN);
        }
        else if (direction == STEPPER_STOP)
        {
            motor->direction = STEPPER_STOP; // 停止
            // 停止时可以保持当前方向或设置默认方向
            // DL_GPIO_clearPins(Stepper_Motor_PORT, Stepper_Motor_DIR1_PIN); // 可选
        }
    }
    else if (motor -> ID == 1)
    {
        if (direction == STEPPER_BACK || direction == 0)
        {
            motor->direction = STEPPER_BACK; // 反转
            DL_GPIO_clearPins(Stepper_Motor_PORT, Stepper_Motor_DIR2_PIN);
        }
        else if (direction == STEPPER_FRONT || direction == 1)
        {
            motor->direction = STEPPER_FRONT; // 正转
            DL_GPIO_setPins(Stepper_Motor_PORT, Stepper_Motor_DIR2_PIN);
        }
        else if (direction == STEPPER_STOP)
        {
            motor->direction = STEPPER_STOP; // 停止
            // 停止时可以保持当前方向或设置默认方向
            // DL_GPIO_clearPins(Stepper_Motor_PORT, Stepper_Motor_DIR2_PIN); // 可选
        }
    }
}




void Disable_StepperMotor(StepperMotor *motor)
{
    motor->Enable = 0; // 禁用
    motor->direction = STEPPER_STOP; // 停止
    StepperMotor_SetDirection(motor, STEPPER_STOP); // 设置方向为停止
    StepperMotor_SetPWMFreq(motor, 0); // 设置PWM频率为0
}
void StepperMotor_SetPWMFreq(StepperMotor *motor, float freq)
{
    // 获取定时器输入时钟（Hz）
    const uint32_t timer_clk = (motor->ID == 0) ? PWM_A_INST_CLK_FREQ : PWM_B_INST_CLK_FREQ;
    // 获取分频因子
    const uint32_t clock_divider = 8;
    const uint32_t clock_prescale = 8;
    // 计算周期寄存器值
    uint32_t new_period_count = 0;
    if (freq != 0) new_period_count = (uint32_t)(timer_clk / (clock_divider * clock_prescale * freq));
    
    
    DL_Timer_setLoadValue((motor->ID == 0) ? PWM_A_INST : PWM_B_INST, new_period_count);
    // 建议同步调整占空比
    DL_Timer_setCaptureCompareValue((motor->ID == 0) ? PWM_A_INST : PWM_B_INST,new_period_count/2, DL_TIMER_CC_0_INDEX); // 50%占空比
}

//下方电机speed为正时，摄像头右移
//上方电机speed为正时，摄像头上移

void StepperMotor_SetSpeed(StepperMotor *motor, float speed,uint8_t un_limit)   //度/秒
{
    
    //速度限幅
    if (un_limit == 0) // 如果不允许超出限幅
    {
        if (speed > SPEED_LIMIT) speed = SPEED_LIMIT;
        else if (speed < -SPEED_LIMIT) speed = -SPEED_LIMIT;
    }
    

    motor->stepper_speed = speed;
    if (motor -> ID == 0)
    {
        if (speed > 0 && StepperCnt_buff[0]>= STEEPER_MOTOR_A_CNT_MAX)
        {
            motor->Enable = 0; // 禁用
            StepperMotor_SetSpeed(motor, 0.0f,0); // 设置速度为0
            return;
        }
        else if (speed < 0 && StepperCnt_buff[0] <= STEEPER_MOTOR_A_CNT_MIN)
        {
            motor->Enable = 0; // 禁用
            StepperMotor_SetSpeed(motor, 0.0f,0); // 设置速度为0
            return;
        }
        if (speed >0)
        {
            motor->Enable = 1; // 使能
            motor->direction = STEPPER_FRONT; // 正转
            StepperMotor_SetDirection(motor, STEPPER_FRONT); // 设置方向为正转
            StepperMotor_SetPWMFreq(motor, speed / STEP_ANGLE_DIVIDED);
        }
        else if (speed < 0)
        {
            motor->Enable = 1; // 使能
            motor->direction = STEPPER_BACK; // 反转
            StepperMotor_SetDirection(motor, STEPPER_BACK); // 设置方向为反转
            StepperMotor_SetPWMFreq(motor, -speed / STEP_ANGLE_DIVIDED);
        }
        else if (speed == 0)
        {
            Disable_StepperMotor(motor); // 禁用电机
        }
    }
    else if (motor -> ID == 1)
    {
        if (speed > 0 && StepperCnt_buff[1] >= STEEPER_MOTOR_B_CNT_MAX)
        {
            motor->Enable = 0; // 禁用
            StepperMotor_SetSpeed(motor, 0.0f,0); // 设置速度为0
            return;
        }
        else if (speed < 0 && StepperCnt_buff[1] <= STEEPER_MOTOR_B_CNT_MIN)
        {
            motor->Enable = 0; // 禁用
            StepperMotor_SetSpeed(motor, 0.0f,0); // 设置速度为0
            return;
        }
        if (speed > 0)
        {
            motor->Enable = 1; // 使能
            motor->direction = STEPPER_FRONT; // 正转
            StepperMotor_SetDirection(motor, STEPPER_FRONT); // 设置方向为正转
            StepperMotor_SetPWMFreq(motor, speed / STEP_ANGLE_DIVIDED);
        }
        else if (speed < 0)
        {
            motor->Enable = 1; // 使能
            motor->direction = STEPPER_BACK; // 反转
            StepperMotor_SetDirection(motor, STEPPER_BACK); // 设置方向为反转
            StepperMotor_SetPWMFreq(motor, -speed / STEP_ANGLE_DIVIDED);
        }
        else if (speed == 0)
        {
            Disable_StepperMotor(motor); // 禁用电机
        }
    }
    
}





void StepperMotor_init(StepperMotor *motor)
{
    motor->stepper_angle = 0;
    motor->stepper_speed = 0;
    motor->Enable = 1; // 使能
}
